#include <QByteArray>
#include <QXmlAttributes>
#include <QXmlStreamReader>
#include <QString>
#include <QDebug>
#include <QDateTime>

#include "_Interface/_global_datastruct.h"
#include "backinstance.h"
#include "Package/oxfordlasersensordll_global.h"
#include "Package/simpletcpsocketdll_global.h"
#include "Package/SimpleRoboticMath/ConsoleMain/simpleroboticmath.h"

void backinstance::frLaserSensorReceive(){
    if(this->pOxfordLaserSensor != nullptr){
        this->pOxfordLaserSensor->LaserSensorLoopRunReceive();
        if(this->pOxfordLaserSensor->isError() == 0){
            this->pLaserSensorData->bValue = true;
            this->pLaserSensorData->x = 0;
            this->pOxfordLaserSensor->GetYDataOnLaserCoordinate(this->pLaserSensorData->y);
            this->pOxfordLaserSensor->GetZDataOnLaserCoordinate(this->pLaserSensorData->z);
            this->pLaserSensorData->a = 0;
            this->pLaserSensorData->b = 0;
            this->pLaserSensorData->c = 0;
        }
        else{
            this->pLaserSensorData->bValue = false;
            this->pLaserSensorData->x = 0;
            this->pLaserSensorData->y = 0;
            this->pLaserSensorData->z = 0;
            this->pLaserSensorData->a = 0;
            this->pLaserSensorData->b = 0;
            this->pLaserSensorData->c = 0;
        }
        this->pLaserSensorData->bSend = true;
    }
}

void backinstance::toLaserSensorSend(){
    double TimeKey;
    static double LaserSendTimeKey = 0;
    TimeKey = QDateTime::currentDateTime().toMSecsSinceEpoch();

    if(this->pOxfordLaserSensor != nullptr){
        if(this->pLaserSensorData->bConnected == true){
            if(this->pLaserSensorData->bSend == true || this->pLaserSensorData->bFirstConnected == true){
                if(TimeKey - LaserSendTimeKey > 25){
                    this->pOxfordLaserSensor->LaserSensorLoopRunSend();
                    this->pLaserSensorData->bSend = false;
                    this->pLaserSensorData->bFirstConnected = false;
                    LaserSendTimeKey = TimeKey;
                }
            }
            else if(this->pLaserSensorData->bSend == false || this->pLaserSensorData->bFirstConnected == false){
                    if(TimeKey - LaserSendTimeKey > 100){
                        this->pOxfordLaserSensor->LaserSensorLoopRunSend();
                        this->pLaserSensorData->bSend = false;
                        this->pLaserSensorData->bFirstConnected = false;
                        LaserSendTimeKey = TimeKey;
                    }
            }
        }
    }
}

void backinstance::ReadLaserSensorDataY(double& value){
    value = this->pLaserSensorData->y;
}

void backinstance::ReadLaserSensorDataZ(double& value){
    value = this->pLaserSensorData->z;
}

bool backinstance::ReadLaserSensorDataValue(){
    return this->pLaserSensorData->bValue;
}
